#include "utility.h"

Eigen::MatrixXd calculateD(){
    Eigen::MatrixXd m(3,3);
    double factor = E*1.0/(1-GAMA*GAMA);
    m(0,0) = 1; m(0,1) = GAMA; m(0,2) = 0;
    m(1,0) = GAMA; m(1,1) = 1; m(1,2) = 0;
    m(2,0) = 0; m(2,1) = 0; m(2,2) = (1-GAMA)/2;
    m *= factor;
    return m;
}

Eigen::MatrixXd calculateB(double x1, double y1, double x2, double y2, double x3, double y3, double x4, double y4, double s, double t){
    Eigen::MatrixXd B(3,8);
    double a,b,c,d;
    a = (y1*(s-1)+y2*(-1-s)+y3*(1+s)+y4*(1-s))/4;
    b = (y1*(t-1)+y2*(1-t)+y3*(1+t)+y4*(-1-t))/4;
    c = (x1*(t-1)+x2*(1-t)+x3*(1+t)+x4*(-1-t))/4;
    d = (x1*(s-1)+x2*(-1-s)+x3*(1+s)+x4*(1-s))/4;
    B(0,0) = a*(t-1)/4-b*(s-1)/4; B(0,1) = 0;
    B(1,0) = 0; B(1,1) = c*(s-1)/4-d*(t-1)/4;
    B(2,0) = B(1,1); B(2,1) = B(0,0);

    B(0,2) = a*(1-t)/4-b*(-s-1)/4; B(0,3) = 0;
    B(1,2) = 0; B(1,3) = c*(-s-1)/4-d*(1-t)/4;
    B(2,2) = B(1,3); B(2,3) = B(0,2);

    B(0,4) = a*(t+1)/4-b*(s+1)/4; B(0,5) = 0;
    B(1,4) = 0; B(1,5) = c*(s+1)/4-d*(t+1)/4;
    B(2,4) = B(1,5); B(2,5) = B(0,4);

    B(0,6) = a*(-t-1)/4-b*(1-s)/4; B(0,7) = 0;
    B(1,6) = 0; B(1,7) = c*(1-s)/4-d*(-t-1)/4;
    B(2,6) = B(1,7); B(2,7) = B(0,6);

    double factor = JacobiMatrix(x1,y1,x2,y2,x3,y3,x4,y4,s,t);
    B = B/factor;
    return B; 
}

double JacobiMatrix(double x1, double y1, double x2, double y2, double x3, double y3, double x4, double y4, double s, double t){
    Eigen::MatrixXd j(4,4);
    j(0,0) = 0; j(0,1) = 1-t; j(0,2) = t-s; j(0,3) = s-1;
    j(1,0) = t-1; j(1,1) = 0; j(1,2) = s+1; j(1,3) = -s-t;
    j(2,0) = s-t; j(2,1) = -s-1; j(2,2) = 0; j(2,3) = t+1;
    j(3,0) = 1-s; j(3,1) = s+t; j(3,2) = -t-1; j(3,3) = 0;
    Eigen::VectorXd x(4);
    Eigen::VectorXd y(4);
    x(0) = x1; x(1) = x2; x(2) = x3; x(3) = x4;
    y(0) = y1; y(1) = y2; y(2) = y3; y(3) = y4;
    Eigen::MatrixXd out(1,1);
    out = x.transpose()*j*y;
    //cout<<out(0,0)/8<<endl;
    return out(0,0)/8;
}

Eigen::MatrixXd calculateK(double x1, double y1, double x2, double y2, double x3, double y3, double x4, double y4){
    Eigen::MatrixXd K = Eigen::MatrixXd::Zero(8,8);
    double s0[4],t0[4];
    s0[0] = 0.57735; s0[1] = 0.57735; s0[2] = -0.57735; s0[3] = -0.57735;
    t0[0] = 0.57735; t0[1] = -0.57735; t0[2] = 0.57735; t0[3] = -0.57735;
    for(int i=0;i<4;i++){
        Eigen::MatrixXd B = calculateB(x1,y1,x2,y2,x3,y3,x4,y4,s0[i],t0[i]);
        Eigen::MatrixXd D = calculateD();
        double factor = H*JacobiMatrix(x1,y1,x2,y2,x3,y3,x4,y4,s0[i],t0[i]);
        Eigen::MatrixXd tmp = B.transpose()*D*B;
        // cout << "tmp" << endl;
        // cout << tmp << endl;
        K += tmp*factor;
    }
    return K;
}
